#include <geometry_msgs/Twist.h>
#include <pantilt_control/PtuGotoAction.h>
#include "ros/ros.h"
#include "actionlib/client/simple_action_client.h"
#include <pantilt_control/PtuGotoAction.h>

typedef actionlib::SimpleActionClient<pantilt_control::PtuGotoAction> ptuGotoClient;

class Pantilt
{

public:
    Pantilt(ros::NodeHandle *nH);
    ~Pantilt();
    void init(bool enabled);
    void set_pantilt_withOutWait(double pan, double tilt);
    bool set_pantilt(double pan, double tilt);

private:
    ptuGotoClient *ptuGoto_ac;
    pantilt_control::PtuGotoGoal ptuGotoGoal;
    ros::NodeHandle *n;
};
Pantilt::Pantilt(ros::NodeHandle *nH):
    n(nH)
{
}

Pantilt::~Pantilt()
{
    delete ptuGoto_ac;
}

void Pantilt::init(bool enabled)
{
    ptuGoto_ac = new ptuGotoClient("SetPTUState",true);
    if(enabled)
        ptuGoto_ac->waitForServer();
    ROS_INFO(" initialize Pantilt action ");
}

void Pantilt::set_pantilt_withOutWait(double pan, double tilt)
{
    ptuGotoGoal.tilt = tilt;
    ptuGotoGoal.pan  = pan;
    ptuGoto_ac->sendGoal(ptuGotoGoal);
}

bool Pantilt::set_pantilt(double pan, double tilt)
{
    ptuGotoGoal.tilt = tilt;
    ptuGotoGoal.pan  = pan;
    ptuGoto_ac->sendGoal(ptuGotoGoal);
    return ptuGoto_ac->waitForResult();
}



int main(int argc, char**argv)
{
    ros::init(argc,argv,"robina_ptuMoveHead");
    ros::NodeHandle nh;
    Pantilt pantilt(&nh);
    pantilt.init(true);
    ros::Rate rate(1000);
    while(ros::ok())
    {
        geometry_msgs::Twist twist = *ros::topic::waitForMessage<geometry_msgs::Twist>("/cmd_vel");
        if(twist.linear.x != 0)
        {
            pantilt.set_pantilt_withOutWait( twist.angular.z*50,
                                             ((twist.linear.x*100) > 0)?((twist.linear.x*100)):0 );
        }
        else if(twist.angular.z != 0)
        {
            pantilt.set_pantilt_withOutWait(twist.angular.z*50,20);
        }

        ros::spinOnce();
        rate.sleep();
    }
    return 0;
}
